
/*
 Step motor demo

 Created 19 Nov. 2020
 by Cedar Diao.
 */

#include <Stepper.h>

// 配置步进电机的最大速度和最小速度，单位rpm。
#define MIN_SPEED 20    // 最低速度 20RPM
#define MAX_SPEED 4000 // 最高速度 4000RPM

// 此步进电机每走一圈的步数
// 没有减速齿轮的电机一般一圈是20步
// 通常是指加了减速齿轮后的步数，即：走几步后轴能转动一圈
// 典型值200 （步幅1.8度，200步就是360度）
const int stepsPerRevolution = 20;

// 配置驱动引脚
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);

void setup() {
  Serial.end();
  Serial.begin(115200);
}

// 电机速度，单位RPM
int motorSpeed = 0;

// 电机方向， 1: 顺时针， -1: 逆时针
#define DIR_CW    (1)  // Clockwise
#define DIR_CCW   (-1) // CounterClockwise

int motorDir = DIR_CW;

// 模拟自动挡汽车挡位: P/R/N/D
// P - 停车挡，马达会停转
// R - 倒车档，马达逆时针运转
// N - 空挡，马达慢慢减速直到停止
// D - 前进挡，马达顺时针运转
char motorLevel = 'N'; // 当前挡位

// 在串口上输出马达的当前速度，单位rpm
static void report_motor_speed(void)
{
  Serial.print("Speed: ");
  Serial.print(motorSpeed, DEC);
  Serial.print("\r\n");
}

// 调整马达速度
// 规则：
//  - 加速不超过最大速度限制
//  - 减速不会低于0（停转）
//  - 减速时，速度低于最小速度会停止
static void motor_speed_change(int deltaSpeed)
{
  if (deltaSpeed > 0) // 加速
  {
    if (motorSpeed < MAX_SPEED)
    {
      motorSpeed += deltaSpeed;

      if (motorSpeed < MIN_SPEED)
      {
        motorSpeed = MIN_SPEED;
      }

      if (motorSpeed > MAX_SPEED)
      {
        motorSpeed = MAX_SPEED;
      }

      report_motor_speed();
    }
  }
  else if (deltaSpeed < 0) // 减速
  {
    if (motorSpeed > 0)
    {
      motorSpeed += deltaSpeed;

      if (motorSpeed < MIN_SPEED)
      {
        motorSpeed = 0;
      }

      report_motor_speed();
    }
  }

  if (motorSpeed > 0)
  {
    myStepper.setSpeed(motorSpeed);
  }
}

#define ACC_STEP   20 // 油门强度，每次加速多少RPM
#define BREAK_STEP (-40) // 刹车强度，每次减速多少RPM

// 换挡逻辑：
// N -> P/R/D
// P -> N/R/D
// D -> N
// R -> N
void loop() {
  int ctrl = Serial.read();

  switch(motorLevel)
  {
    case 'D': // 前进
    if (ctrl == 'N') // D -> N
    {
      motorLevel = 'N';
      Serial.println("D -> N");
    }
    else if (ctrl == '+') // ACC
    {
      motor_speed_change(ACC_STEP);
    }
    else if (ctrl == '-') // 刹车
    {
      motor_speed_change(BREAK_STEP);
    }
    break;

    case 'N': // 空挡，逐渐减速
    if (ctrl == 'D')
    {
      motorDir = DIR_CW;
      motorLevel = 'D';
      Serial.println("N -> D");

      if (motorSpeed == 0)
      {
        motor_speed_change(MIN_SPEED);
      }
    }
    else if (ctrl == 'R')
    {
      motorDir = DIR_CCW;
      motorLevel = 'R';
      Serial.println("N -> R");

      if (motorSpeed == 0)
      {
        motor_speed_change(MIN_SPEED);
      }
    }
    else if (ctrl == 'P')
    {
      motor_speed_change((-1)*MAX_SPEED);
      motorLevel = 'P';
      Serial.println("N -> P");
    }
    else if (ctrl == '-')
    {
      motor_speed_change(BREAK_STEP);
    }
    else
    {
      motor_speed_change(-1);
    }    
    break;

    case 'R': // 倒退
    if (ctrl == 'N') // R -> N
    {
      motorLevel = 'N';
      Serial.println("R -> N");
    }
    else if (ctrl == '+') // ACC
    {
      motor_speed_change(ACC_STEP);
    }
    else if (ctrl == '-') // 刹车
    {
      motor_speed_change(BREAK_STEP);
    }
    break;

    case 'P': // 停止
    if (ctrl == 'D')
    {
      motorDir = DIR_CW;
      motorLevel = 'D';
      Serial.println("P -> D");

      if (motorSpeed == 0)
      {
        motor_speed_change(MIN_SPEED);
      }
    }
    else if (ctrl == 'R')
    {
      motorDir = DIR_CCW;
      motorLevel = 'R';
      Serial.println("P -> R");

      if (motorSpeed == 0)
      {
        motor_speed_change(MIN_SPEED);
      }
    }
    else if (ctrl == 'N')
    {
      Serial.println("P -> N");
    }
    break;
  }

  if (motorSpeed > 0)
  {
    myStepper.step(motorDir*1);
  }  
}
